/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
import lejos.nxt.*;
import lejos.nxt.comm.Bluetooth;
//import lejos.pc.comm.*;
import java.io.IOException;

import lejos.nxt.*;
import lejos.nxt.comm.*;
import java.util.*;
import java.lang.*;
import java.io.*;


/**
 *  If the Robot loses contact with the Client, it will enter this Class and
 *  perform operations until contact is re-established.  It will enter 
 *  complete recycling mode while trying to re-establish contact.
 *
 *
 * @author by Bobby
 */
public class AI{
	
	static boolean tryConnecting = false;
	
	int searchCount;
	
	Control controlAI;
	
	BT bluetoothAI;
	
	Touch touchAI;
	
	UpdateConnect upConAI;
	
	UltrasonicSensor sensor;

	final int ROBOTRANGE = 100;
    
	//String BTDeviceName;
	
	/*public void run(){
		upConAI.start();
    	try{
    		this.updateConnection();
    		this.startAI();
    	}catch (Exception e){
    	
    	}
		
	}*/
    
    /**
     *  Used to begin the AI finite state machine process.
     * 
     */ 
    public void startAI(Control pControlAI, Touch pTouchAI, BT pBluetoothAI)throws Exception{
    	
    	bluetoothAI = pBluetoothAI;
    	controlAI = pControlAI;
    	touchAI = pTouchAI;
    	searchCount = 0;
    	if(!this.tryConnecting){
    		LCD.refresh();
    		LCD.drawString("Starting UpConnection", 0, 0);
    		UpdateConnect upConnect = new UpdateConnect(bluetoothAI);
    		upConnect.start();
    		this.tryConnecting = true;
    	}
    	
    	LCD.clear();
    	LCD.drawString("Starting AI...",0,0);
    	LCD.refresh();
    	
    	Thread.sleep(500); // sleeping
    	
		controlAI.doCommand("right");
		
    	Thread.sleep(500); // sleeping
		   
    	   
    	controlAI.doCommand("left");
    	Thread.sleep(500);
    	controlAI.doCommand("stop");
    	
    		robotAiEngage();		 	
    }
    
    

    
    private void robotAiEngage() throws Exception{
    	
    	LCD.clear();
    	LCD.drawString("Entering Robot Engage",0,0);
    	LCD.refresh();
    	
    		robotAiSearchMode();			
    }


	/**
	 * robotAiSearchMode.  This static method is used to search for an object to recycle.
	 * If an object is found, it will move towards it.  Once it is close enough it will
	 * engage the recycle mode.
	 * 
	 */
	private void robotAiSearchMode() throws Exception{
		sensor = new UltrasonicSensor(SensorPort.S3);
		
		
		
		LCD.drawString("Entering Search Mode",0,0);
		Thread.sleep(1000);
		//If object is not in range, rotate right
		if (!bluetoothAI.isConnected()){
			
		}
		else if (!isObjectinRange() &&  searchCount < 12)
		{
			searchCount++;
			controlAI.doCommand("right");
			
			Thread.sleep(80);				
			controlAI.doCommand("stop");
			if(!bluetoothAI.isConnected())
				robotAiEngage();
		}


		// if Object is in range, move towards the object
		else if (isObjectinRange())
		{
			searchCount = 0;
			if (sensor.getDistance() <= 40) // 40 is arbitrary not sure what exact value will be
			{
					robotAiRecycleMode();
			}
			else
			{
				controlAI.doCommand("forward");
				Thread.sleep(500);	
				controlAI.doCommand("stop");
				
				robotAiEngage();	
			}
		}else if (searchCount > 11){
			searchCount = 0;
			controlAI.doCommand("forward");
			Thread.sleep(2000);
			controlAI.doCommand("left");
			Thread.sleep(500);
			controlAI.doCommand("forward");
			Thread.sleep(1500);
			controlAI.doCommand("stop");
			
			robotAiEngage();
		}
	}

	/*
	 * robotAiRecycleMode.  This method is used to enter the recycle Mode of the robot.  It will
	 * grab the can and return it to the recycle point.  This method still needs some work.
	 * 
	*/
	private void robotAiRecycleMode() throws Exception {
			int sleepAmt = 0;
			// Need to implement commands to move to the recycle point and drop the can
			
			LCD.clear();
			LCD.drawString("Moving Forward",0,0);
			LCD.refresh();
			
			sleepAmt = sensor.getDistance() *50;
    		controlAI.doCommand("forward");
    		
    		Thread.sleep(sleepAmt);
            controlAI.doCommand("stop");
            
            
            controlAI.doCommand("closegrip");
            int i = 0;
            while(!touchAI.getIsPressed() && i != 5000){
            	i++;
            }
            Motor.C.stop();
            
            
            if (!touchAI.getIsPressed()){
            	controlAI.doCommand("opengrip");
            	Thread.sleep(500);
            	controlAI.doCommand("backward");
            	Thread.sleep(1000);
            	controlAI.doCommand("stop");
            	if(!bluetoothAI.isConnected()) 
            		robotAiSearchMode();
            }else{
          
            	controlAI.doCommand("backward");
            	Thread.sleep(3000);
            	controlAI.doCommand("stop");
				controlAI.doCommand("left");
				Thread.sleep(800);
	    		controlAI.doCommand("forward");
	    		Thread.sleep(2000);
	    		controlAI.doCommand("stop");
	    		controlAI.doCommand("opengrip");

	    		Thread.sleep(1000);
	    		controlAI.doCommand("backward");
	    		Thread.sleep(2000);
	    		controlAI.doCommand("right");
	    		Thread.sleep(800);
	    		controlAI.doCommand("forward");
	    		Thread.sleep(3000);
	    		controlAI.doCommand("stop");
	    		
            }
	}


	/**
	 * Simple method checking to see if a can or bottle is in range of the Robot
	 * Will detect walls and other objects too, cannot determine objects, just if
	 * an object is in front of it.
	 *
	 * @return boolean
	 */

	
	private boolean isObjectinRange()throws Exception{
		
		
		
		LCD.clear();
		LCD.drawString("Checking if object",0,0);
		LCD.drawString("is in Range", 0, 1);
		LCD.refresh();
		Thread.sleep(1000);
		LCD.clear();
		LCD.drawString(sensor.getVersion(), 0, 0);
		LCD.drawString(sensor.getProductID(), 0, 1);
		LCD.drawString(sensor.getSensorType(), 0, 2);
		Thread.sleep(1000);
		LCD.drawInt(sensor.getDistance(), 0, 3);
		LCD.refresh();
		Thread.sleep(1000);
		
		
		
		boolean inRange = false;

		
		Thread.sleep(500);
		
		if(sensor.getDistance() < ROBOTRANGE){	
			LCD.drawString("Object in RANGE!",0,4);
			LCD.refresh();
			Thread.sleep(1000);
			inRange = true;
		}
		return inRange;
	}
    
}

